%% 测向定位最优布站 %%
clc
clear
close all

% choice   0 遗传算法 1 粒子群算法 2 序列二次规划
choice = 2;
T =  [0 0 0];
% 目标位置约束范围[x_l x_r y_l y_h]
% T_range = [-1 1 -1 1];
% T_range = [-5 5 -5 5];
% T_range = [-10 10 -10 10];
T_range = [-50 10 40 50];
% T_range = [-100 100 70 80];
% choice_range   0 目标固定在一点 1 目标在一定区域内
choice_range = 0;

%% 边界约束 choice_bound  0 不加边界约束 1 加边界约束
choice_bound = 1;
if ~choice_bound
    lb = [];ub = [];
else
%     lb = [-20 -20 -20 -20 -20 -20];
%     ub = [0 0 0 0 0 0];
%     lb = [-100 -10 -100 -10 -100 -10];
%     ub = [100 0 100 0 100 0];
    lb = [-100 -100 -100 -100 -100 -100];
    ub = [100 100 100 100 100 100];
end

% contype 1 等边三角形布站 2 目标与监测站距离约束
contype = 0;
% choice_S0 初始值计算 1 圆  0 矩形顶点
choice_S0 = 1;
% 以目标为圆心 r 为半径的约束
r = 20;

%% 主体 %%
switch choice
    case 0
        if ~choice_range
            [S_min,fval] = GaOpt(r,T,0,lb,ub,contype);
            figure
            plot(T(1),T(2),'*');hold;
            plot([S_min(1) S_min(3) S_min(5)],...
            [S_min(2) S_min(4) S_min(6)],'o','LineWidth',2);hold;
            if contype == 2
                rectangle('Position',[T(1)-r,T(2)-r,2*r,2*r],'Curvature',[1 1],...
                    'EdgeColor','b');
                axis([-100 100 -100 100])
            end
            title(['GA,GDOP_m_i_n = ',num2str(fval)]);
        else 
            [S_min,fval] = GaOpt(r,0,T_range,lb,ub,contype);
            figure
            rectangle('Position',[T_range(1) T_range(3) T_range(2)-T_range(1) T_range(4)-T_range(3)],...
            'EdgeColor','b','FaceColor','c');hold on;
            rectangle('Position',[lb(1) lb(2) ub(1)-lb(1) ub(2)-lb(2)],...
            'EdgeColor','b','FaceColor','g');
            plot([S_min(1) S_min(3) S_min(5)],...
            [S_min(2) S_min(4) S_min(6)],'o','LineWidth',2);hold off;
            title(['PSO,GDOP_a_v_g = ',num2str(fval)]);
            axis([-100 100 -100 100])
        end
    case 1
        [S_min,fval] = PSO(T_range,lb,ub);
        figure
        rectangle('Position',[T_range(1) T_range(3) T_range(2)-T_range(1) T_range(4)-T_range(3)],...
        'EdgeColor','b','FaceColor','c');hold on;
        if ~choice_S0
            rectangle('Position',[lb(1) lb(2) ub(1)-lb(1) ub(2)-lb(2)],...
            'EdgeColor','b','FaceColor','g');
            axis([-100 100 -100 100])
        end
        plot([S_min(1) S_min(3) S_min(5)],...
        [S_min(2) S_min(4) S_min(6)],'o','LineWidth',2);hold off;
        title(['PSO,GDOP_a_v_g = ',num2str(fval)]);
    case 2 
        if choice_S0 
            alpha1 = 5*pi/180;
            alpha2 = alpha1+100*pi/180;
            alpha3 = alpha2+100*pi/180;
            R = r + 10;
            S10 = [T(1)+R*cos(alpha1) T(2)+R*sin(alpha1)];
            S20 = [T(1)+R*cos(alpha2) T(2)+R*sin(alpha2)];
            S30 = [T(1)+R*cos(alpha3) T(2)+R*sin(alpha3)];
            S0 = [S10 S20 S30];
        else
            S0 = [(lb(1)+ub(1))/2 (lb(2)+ub(2))/2 lb(1) lb(2) ub(1) ub(2)];
        end
        if ~choice_range
            [S_min,fval] = SqpOpt(r,T,0,lb,ub,S0,contype);
            figure
            plot(T(1),T(2),'*');hold on;
            plot([S_min(1) S_min(3) S_min(5)],...
            [S_min(2) S_min(4) S_min(6)],'o','LineWidth',2);
            if contype == 2
                rectangle('Position',[T(1)-r,T(2)-r,2*r,2*r],'Curvature',[1 1],...
                    'EdgeColor','b');
                plot([S0(1) S0(3) S0(5)],[S0(2) S0(4) S0(6)],'o');
                axis([-100 100 -100 100])
            end
            hold off;
            title(['SQP,GDOP_m_i_n = ',num2str(fval)]);
        else
            [S_min,fval] = SqpOpt(r,0,T_range,lb,ub,S0,contype);
            figure
            rectangle('Position',[T_range(1) T_range(3) T_range(2)-T_range(1) T_range(4)-T_range(3)],...
            'EdgeColor','b','FaceColor','c');hold on;
            if ~choice_S0
                rectangle('Position',[lb(1) lb(2) ub(1)-lb(1) ub(2)-lb(2)],...
                'EdgeColor','b','FaceColor','g');
            end
            plot([S_min(1) S_min(3) S_min(5)],...
            [S_min(2) S_min(4) S_min(6)],'o','LineWidth',2);hold off;
            title(['SQP,GDOP_a_v_g = ',num2str(fval)]);
            if ~choice_S0
                axis([-100 100 -100 100])
            end
        end
end
